#include <stdio.h>

#include "common.h"
#include "communicate.h"
#include "uart.h"
#include "canbus.h"
#include "ethserver.h"
#include "robot.h"
#include "dataqueue.h"
#include "control.h"
#include "datamanger.h"
#include "navigation.h"
#include "odom.h"

int32_t handlefd[MAXFD];        //所有通信的文件描述符

/*
 *  初始化所有通讯接口
 *  获取通讯接口文件描述符
 */
void comm_init(void)
{
	int fd;
	handlefd[CANFD] = -1;	//默认为-1,
    handlefd[UARTFD] = -1; 
    handlefd[ETHFD] = -1;
	handlefd[CLIENTFD] = -1;
    handlefd[UDPFD] = -1;

	fd = canbus_init(); //初始化can0,返回套接字句柄
	if(fd > 0){
		handlefd[CANFD] = fd;
        printf("Init: canbus init successfully!\r\n");
	}
    else{
        printf("Init: canbus init failed!\r\n");
    }

    fd = uart_init(); //初始化串口,返回句柄
	if(fd > 0){
		handlefd[UARTFD] = fd;
        printf("Init: uart init successfully!\r\n");
	}
    else{
        printf("Init: uart init failed!\r\n");
    }

	fd = tcpserver_init();
	if(fd > 0){
		handlefd[ETHFD] = fd;	
        printf("Init: tcpserver init successfully!\r\n");
	}
    else{
        printf("Init: tcpserver init failed!\r\n");
    }

    fd = udpserver_init();
    if(fd > 0){
        handlefd[UDPFD] = fd;
        printf("Init: udpserver init successfully!\r\n");
    }
    else{
        printf("Init: udpserver init failed!\r\n");
    }
    
}


void recv_eth_data(const char *buff, const unsigned int len)
{
	int8_t ret=0;
    int i;
    uint8_t recvdata;		// tcp recv a char in queque
	
    
	static uint8_t serverrecvstat = SERVER_HEAD1;	//接收状态
	static uint8_t serverlen = 0;				//已经接收的长度
	static uint8_t serverchecksum = 0;			//校验和
	static ServerRecvCmd_t serverrecvbuf;

    for (i=0; i< len ; i++){
        recvdata = buff[i];
		switch ( serverrecvstat )
		{
			case SERVER_HEAD1:
				if( recvdata == 0xFF)
				{
					serverrecvstat = SERVER_HEAD2;
				}
				break;
			case SERVER_HEAD2:
				if( recvdata == 0xAA )
				{
					serverrecvstat = SERVER_LEN;
				}
				else if( recvdata == 0xFF )
				{
					// after one err , sync sucess
					serverrecvstat = SERVER_HEAD2;
				}
				else
				{
					// after one err , sync fail
					serverrecvstat = SERVER_HEAD1;
				}
				break;
			case SERVER_LEN:
				serverrecvstat = SERVER_OPERATE;
				serverchecksum = recvdata;
				serverlen = 0;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_OPERATE:
				serverrecvstat = SERVER_DEVICETYPE;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DEVICETYPE:
				serverrecvstat = SERVER_DEVICENAME;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DEVICENAME:
				serverrecvstat = SERVER_DATA;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DATA:
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				if( serverlen >= ETH_CMD_LENGTH - 1)
				{
					serverrecvstat = SERVER_CHECK;
				}
				break;
			case SERVER_CHECK:
				if(serverchecksum == recvdata)
				{
                    //解析到一帧完整数据
					eth_add_task(ethrecvqueue,serverrecvbuf.RecvData);    //添加到接收队
				}
				serverrecvstat = SERVER_HEAD1;
				break;
			default:
				serverrecvstat = SERVER_HEAD1;
				break;
		}
    }

}

void servertaskdistribute(ServerCmdStruct *RecvDataBuff)
{

	uint8_t devicetype;
    uint8_t devicename;
    DATA32_t devicedata;

    ROBOT_POSITION  position;

	// printf("len:        %x\n",RecvDataBuff->Len);
	// printf("Operate:    %x\n",RecvDataBuff->Operate);
	// printf("deviceType: %x\n",RecvDataBuff->DeviceType);
	// printf("DeviceName: %x\n",RecvDataBuff->DeviceName);
	// printf("Data:       %x %x %x %x\n",RecvDataBuff->Data.raw[0], RecvDataBuff->Data.raw[1],RecvDataBuff->Data.raw[2],RecvDataBuff->Data.raw[3]);

	devicetype = RecvDataBuff->DeviceType;
    devicename = RecvDataBuff->DeviceName;
    devicedata = RecvDataBuff->Data;
	if( devicetype == ALL_DEVICES )
	{	
		if( devicetype == MOTORINIT )	//系统心跳
		{
			//ServerSendPackage(ISREPLY, devicetype, devicename,0);
		}
	}	
    else if(devicetype == ROBOT_ACTION)	//机器人动作控制
	{
		RobotActionCTRL((ACTION_TYPE) devicename, devicedata.data32);
	}
	else if(devicetype == NVIDIA_BACK)	//NVIDIA 识别结果数据
	{
		nvidia_handle(devicename,devicedata);
	}	
    else if(devicetype == ROBOT_ODOM)
    {
        read_odom(&position);
        //printf("odom dual: %d; distance: %d\n",position.dual, position.dist_to_radpoint);
    }
	else if(devicetype == TING_KAO)		//控制停靠点是否停靠
	{
		//TingKao_Sign(devicename, devicedata);		
	}
	else if(devicetype == ROBOT_CHARGE)	//充电，区分在充电点，不在充点电
	{	
		//come_back(devicedata.data32);
	}
	else if(devicetype == AUTO_AROUND) 	//一键自动巡检
	{
		auto_arount(devicedata.data32);	
	}
	else if(devicetype == ROBOT_LINEWALKING)	//巡线使能指令
	{
		if(devicename){	
			robotinfo_write(LINE_WARNING, 1);
		}else{
			robotinfo_write(LINE_WARNING, 0);
		}
        //printf("line walking state changed!\r\n");
	}
	else if(devicetype == TIME_STATE){	//巡检灯的控制
        //trigger_event(EVENT_GO_DISTANCE, -60, 0, 0);
		//trigger_event(EVENT_TURN_AROUND, 90, 45, 0);


	}
    else if(devicetype == TEST_CMD){    //测试风扇
        // four_fans_ctrl(devicename);
        // navigation_led_ctrl(devicename);
        // power_yuntai_ctrl(devicename);
        // navigation_camera_ctrl(devicename);
		write_odom(45,2351);		//里程计清理
        
    }

	else{
		//没有相应的设备
		//执行到这里的，肯定任务分发失败了
        printf("Eth Cmd devicetype error !!\r\n");
	}
    eth_cmd_package(1,devicetype,devicename,0);
}

void eth_cmd_package(uint8_t operate, uint8_t devicetype, uint8_t devicename,uint32_t data)
{
    ServerRecvCmd_t databuf;

    databuf.RecvDatastruct.Len = ETH_CMD_LENGTH;
    databuf.RecvDatastruct.Operate = operate;
    databuf.RecvDatastruct.DeviceType = devicetype;
    databuf.RecvDatastruct.DeviceName = devicename;
    databuf.RecvDatastruct.Data.data32 = data;

	//发送数据，添加数据到发送队列，等待发送
	eth_add_task(ethsendqueue,databuf.RecvData);
	
}


/*
 * 发送线程调用
 * 调用底层发送函数
 * */
void eth_send_package(uint8_t *buff)
{
    uint8_t databuf[ETH_CMD_LENGTH + 3];
    uint8_t checksum = 0;
    uint8_t i;
    databuf[0] = 0xff; 
    databuf[1] = 0xaa;

    for(i = 0; i < ETH_CMD_LENGTH; i++)
    {
        databuf[i+2] = buff[i];
        checksum += buff[i];
    }
    databuf[10] = checksum;
    eth_writen(databuf,ETH_CMD_LENGTH + 3);

}



void recv_uart_data(const char *buff,const unsigned int len)
{
    int i;
    uint8_t recvdata;		// tcp recv a char in queque
	static uint8_t uartrecvstat = UART_HEAD1;	//接收状态
	static uint8_t uartlen = 0;				//已经接收的长度
	static uint8_t uartchecksum = 0;			//校验和
	static UartRecvCmd_t uartrecvbuf;

    for (i=0; i< len ; i++){
        recvdata = buff[i];
		switch ( uartrecvstat )
		{
			case UART_HEAD1:
				if( recvdata == 0xfe)
				{
					uartrecvstat = UART_HEAD2;
				}              
				break;
			case UART_HEAD2:
				if( recvdata == 0xaa )
				{
					uartrecvstat = UART_OPERATE;
				}
				else if( recvdata == 0xfe )
				{
					// after one err , sync sucess
					uartrecvstat = UART_HEAD2;
				}
				else
				{
					uartrecvstat = UART_HEAD1;
				}
				break;
			case UART_OPERATE:
				uartrecvstat = UART_TYPE;
				uartchecksum = recvdata;
				uartlen = 0;
				uartrecvbuf.cmddata[uartlen] = recvdata;
				break;
			case UART_TYPE:
				uartrecvstat = UART_DATA;
				uartchecksum += recvdata;
				uartlen++;
				uartrecvbuf.cmddata[uartlen] = recvdata;
				break;
			case UART_DATA:
				uartchecksum += recvdata;
				uartlen++;
				uartrecvbuf.cmddata[uartlen] = recvdata;
                if(uartlen >= 3){
                    uartrecvstat = UART_CHACK;  
                }
				break;
			case UART_CHACK:
				if(uartchecksum == recvdata)
				{
                    //解析到一帧完整数
					uart_add_task(uartrecvqueue,uartrecvbuf.cmddata);    //添加到接收队
				}
				uartrecvstat = UART_HEAD1;
				break;
			default:
				uartrecvstat = UART_HEAD1;
				break;
		}
    }
}

void uart_updata(UartRecvCmd_t *recvbuff)
{
	uint8_t	operate;
	uint8_t deviceType;
    DATA16_t  data;
	DATAMANGER_INDEX index;
	int32_t temp;

	operate = recvbuff->cmdstruct.operate;
	deviceType = recvbuff->cmdstruct.deviceType;
	data = recvbuff->cmdstruct.data;
	temp = data.data16;
    //数据更性
    //printf("operate: %02x  devicetype: %02x  data: %d\n", operate,deviceType,data.data16);
	switch (recvbuff->cmdstruct.deviceType)
	{
	case POWER_PERCENT_TYPE:
		robotinfo_write(POWER_PER, (int32_t)temp);
		break;
	case GYRO_YZHOU_TYPE:
		robotinfo_write(GYRO, (int32_t) temp);
		break;
	case DISTANCE_RADAR_TYPE:
		robotinfo_write(RADAR, (int32_t) temp);
		break;
	default:
		break;
	}
	

}

void uart_cmd_package(uint8_t operate, int8_t davicetype, int16_t data)
{
    
    UartRecvCmd_t  uartsenddata;

    uartsenddata.cmdstruct.data.data16 = data;
    uartsenddata.cmdstruct.deviceType = davicetype;
    uartsenddata.cmdstruct.operate = operate;

    uart_add_task(uartsendqueue, uartsenddata.cmddata);
}

void uart_send_package(uint8_t *buff)
{
    uint8_t databuf[UART_CMD_LENGTH + 3];
    uint8_t checksum;
    uint8_t i;
    databuf[0] = 0xfe; 
    databuf[1] = 0xaa;

    for(i = 0; i < UART_CMD_LENGTH; i++)
    {
        databuf[i+2] = buff[i];
        checksum += buff[i];
    }
    databuf[10] = checksum;
    uart_wirte(databuf,UART_CMD_LENGTH + 3);
}




void recv_udp_data(const char *buff, const unsigned int len)
{
	int8_t ret = 0;
    int i;
    uint8_t recvdata;		// tcp recv a char in queque

	static uint8_t serverrecvstat = SERVER_HEAD1;	//接收状态
	static uint8_t serverlen = 0;				//已经接收的长度
	static uint8_t serverchecksum = 0;			//校验和
	static ServerRecvCmd_t serverrecvbuf;

    for (i=0; i< len ; i++){
        recvdata = buff[i];
		switch ( serverrecvstat )
		{
			case SERVER_HEAD1:
				if( recvdata == 0xFF)
				{
					serverrecvstat = SERVER_HEAD2;
				}
				break;
			case SERVER_HEAD2:
				if( recvdata == 0xAA )
				{
					serverrecvstat = SERVER_LEN;
				}
				else if( recvdata == 0xFF )
				{
					// after one err , sync sucess
					serverrecvstat = SERVER_HEAD2;
				}
				else
				{
					// after one err , sync fail
					serverrecvstat = SERVER_HEAD1;
				}
				break;
			case SERVER_LEN:
				serverrecvstat = SERVER_OPERATE;
				serverchecksum = recvdata;
				serverlen = 0;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_OPERATE:
				serverrecvstat = SERVER_DEVICETYPE;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DEVICETYPE:
				serverrecvstat = SERVER_DEVICENAME;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DEVICENAME:
				serverrecvstat = SERVER_DATA;
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				break;
			case SERVER_DATA:
				serverchecksum += recvdata;
				serverlen ++;
				serverrecvbuf.RecvData[serverlen] = recvdata;
				if( serverlen >= ETH_CMD_LENGTH - 1)
				{
					serverrecvstat = SERVER_CHECK;
				}
				break;
			case SERVER_CHECK:
				if(serverchecksum == recvdata)
				{
                    //解析到一帧完整数据
					eth_add_task(ethrecvqueue,serverrecvbuf.RecvData);    //添加到接收队
				}
				serverrecvstat = SERVER_HEAD1;
				break;
			default:
				serverrecvstat = SERVER_HEAD1;
				break;
		}
    }

}

void udp_cmd_package(uint8_t destip,uint16_t destport, const uint8_t *data, uint8_t len)
{ 
    udp_add_task(destip, destport, data, len);
}

void udp_send_package(uint8_t destip,uint16_t destport, uint8_t *data, uint8_t len)
{
    udp_send(data,len,destip,destport);//底层发送函数
}








